#include <string.h>
#include <math.h>

#include "triple.h"
#include "Matrix.h"
#include "memory.h"

OPERATOR_NEW_OVERLOAD(Triple)

void Triple::ToHTMLString(ostream& os, char *title)
{
	os << "<table><tbody><tr><th>" << title << "</th><td>" 
		<< coord[X] << "</td><td>"
		<< coord[Y] << "</td><td>"
		<< coord[Z] << "</td></tr></tbody></table>";
}

// default ctor
Triple::Triple()
{
	//__asm { int 3 }
	// coord = xmm0
	const void *dest = this->coord;
	__asm
	{
		mov eax,dest
		movaps xmmword ptr [eax],xmm0
	}
}

Triple::Triple(FP_DATATYPE *src)
{
	for (int i = 0; i < 3; i++)
	{
		this->coord[i] = src[i];
	}
}

void Triple::Set(float x, float y, float z)
{
	coord[X] = x;
	coord[Y] = y;
	coord[Z] = z;
}

void Triple::Reset()
{
	coord[X] = 0.0f;
	coord[Y] = 0.0f;
	coord[Z] = 0.0f;
}

void Triple::operator=(const Triple& src)
{
	for (int i = 0; i < 3; i++)
	{
		this->coord[i] = src.coord[i];
	}
}

void Triple::operator delete(void* m)
{
	Triple *t = reinterpret_cast<Triple*>(m);
	if (t->base_ptr != NULL)
	{
		free(t->base_ptr);
	}
}

// Identifier coord in assembler is considered to be the offset of the coord member in any Triple instance (which is 0 or 4)
const Triple operator+(Triple& left, Triple& right)
{
#ifdef SSSE3
	__asm
	{
		mov eax,left
		movaps xmm0,xmmword ptr [eax]
		mov eax,right
		addps xmm0,xmmword ptr [eax]
	}
	return Triple();
#else
	FP_DATATYPE dest[3];
	for (int i = 0; i < 3; i++)
	{
		dest[i] = left[i] + right[i];
	}
	return Triple(dest);
#endif
}

const void Triple::operator+=(Triple& right)
{
#ifdef SSSE3
	const void *dest = this->coord;
	__asm
	{
		mov eax,right
		movaps xmm0,xmmword ptr [eax]
		mov eax,dest
		addps xmm0,xmmword ptr [eax]
		movaps xmmword ptr [eax],xmm0
	}
#else
	for (int i = 0; i < 3; i++)
	{
		this->coord[i] += right[i];
	}
#endif
}

const Triple operator-(Triple& left, Triple& right)
{
#ifdef SSSE3
	__asm
	{
		mov eax,left
		movaps xmm0,xmmword ptr [eax]
		mov eax,right
		subps xmm0,xmmword ptr [eax]
	}
	return Triple();
#else
	FP_DATATYPE dest[3];
	for (int i = 0; i < 3; i++)
	{
		dest[i] = left[i] - right[i];
	}
	return Triple(dest);
#endif
}

const Triple operator*(Triple& left, Triple& right)
{
#ifdef SSSE3
	__asm
	{
		mov eax,left
		movaps xmm0,xmmword ptr [eax]
		mov eax,right
		mulps xmm0,xmmword ptr [eax]
	}
	return Triple();
#else
	FP_DATATYPE dest[3];
	for (int i = 0; i < 3; i++)
	{
		dest[i] = left[i] * right[i];
	}
	return Triple(dest);
#endif
}

const Triple operator*(Triple& left, const FP_DATATYPE right)
{
#ifdef SSSE3
	__asm
	{
		movss xmm1,right                         ;// xmm1 <= right
		shufps xmm1,xmm1,0x00

		mov eax,left
		movaps xmm0,xmmword ptr [eax]
		mulps xmm0,xmm1
	}
#else
	FP_DATATYPE dest[3];
	for (int i = 0; i < 3; i++)
	{
		dest[i] = left[i] * right;
	}
	return Triple(dest);
#endif
}

void Triple::Multiply(Matrix& m, Triple& t)
{
	coord[X] = m[0].DotProduct(t);
	coord[Y] = m[1].DotProduct(t);
	coord[Z] = m[2].DotProduct(t);
}

void Triple::Integrate(Triple& derivative, const FP_DATATYPE dt)
{
	const void *dest = this->coord;
#ifdef SSSE3_error
	__asm
	{
		movss xmm1,dt                            ;// xmm1 <= dt
		shufps xmm1,xmm1,0x00

		mov eax,derivative
			movaps xmm0, xmmword ptr [eax]       ;// xmm0 <= derivative
			mulps xmm0, xmm1                     ;// xmm0 <= derivative*dt
			mov eax,dest
			addps xmm0, xmmword ptr [eax]        ;// xmm0 <= derivative*dt + destination
			movaps xmmword ptr [eax], xmm0
	}
#else
	coord[X] += derivative[X] * dt;
	coord[Y] += derivative[Y] * dt;
	coord[Z] += derivative[Z] * dt;
#endif
}

const Triple operator/(Triple& left, Triple& right)
{
#ifdef SSSE3
	__asm
	{
		mov eax,left
		movaps xmm0,xmmword ptr [eax]
		mov eax,right
		divps xmm0,xmmword ptr [eax]
	}
	return Triple();
#else
	FP_DATATYPE dest[3];
	for (int i = 0; i < 3; i++)
	{
		dest[i] = left[i] / right[i];
	}
	return Triple(dest);
#endif
}

const Triple operator/(Triple& left, const FP_DATATYPE right)
{
#ifdef SSSE3
	__asm
	{
		movss xmm1,right
        shufps xmm1,xmm1,0x00

		mov eax,left
		movaps xmm0,xmmword ptr [eax]
		divps xmm0,xmm1
	}
#else
	FP_DATATYPE dest[3];
	for (int i = 0; i < 3; i++)
	{
		dest[i] = left[i] / right;
	}
	return Triple(dest);
#endif
}

FP_DATATYPE Triple::DotProduct()
{
	return coord[X]*coord[X] + coord[Y]*coord[Y] + coord[Z]*coord[Z];
}

FP_DATATYPE Triple::DotProduct(Triple& a)
{
#ifdef SSSE3
	float p;
	const void *b = this->coord;
	__asm
	{
		;// load
		mov eax,a
			movaps xmm0,[eax]
		mov eax,b
			mulps xmm0,[eax]

		; // sum
		pshufd xmm1,xmm0,0xE5 ;// put y in place of x (0b11100101)
		pshufd xmm2,xmm0,0xE6 ;// put z in place of x (0b11100110)
		addss xmm0,xmm1
			addss xmm0,xmm2

			; // store
		movss p,xmm0
	}
	return p;
#else
	return this->coord[X]*a[X] + this->coord[Y]*a[Y] + this->coord[Z]*a[Z];
#endif
}

void Triple::CrossProduct(Triple& a, Triple& b)
{
	const void *dest = this->coord;
#ifdef SSSE3
	__asm
	{
		;// load
		mov eax,a
			movaps xmm0,[eax]
		mov eax,b
			movaps xmm1,[eax]

		pshufd xmm2,xmm0,0xC9   ;// shuffle 'a' 0b11001001
		pshufd xmm3,xmm1,0xC9   ;// shuffle 'b' the same way

		mulps xmm0,xmm3         ;// 1-st multiplication, alter 'a'
		mulps xmm1,xmm2         ;// 2-nd multiplication, alter 'b'

		subps xmm0,xmm1

			pshufd xmm1,xmm0,0xC9   ;// shuffle back

		;// store
		mov edx,dest
			movaps [edx],xmm1
	}
#else
	coord[X] = a[1]*b[2] - a[2]*b[1];
	coord[Y] = a[2]*b[0] - a[0]*b[2];
	coord[Z] = a[0]*b[1] - a[1]*b[0];
#endif
}

void Triple::Project(Triple& f, Triple& support)
{
	const FP_DATATYPE s = f.DotProduct(support) / support.DotProduct();
	//multiply__t_s(projection, support, s);
	coord[X] = s*support[X]; // TODO Use method
	coord[Y] = s*support[Y];
	coord[Z] = s*support[Z];
}
/*
void move_triple(const LinearMovement* movement, const triple from, const triple to, triple dest)
{
#ifdef SSSE3
__asm
{
mov eax,from
movaps xmm0,xmmword ptr [eax]            ; xmm0 = from
mov eax,to                               ;
movaps xmm1,xmmword ptr [eax]            ; xmm1 = to
mulps xmm0, xmm5                         ;
mulps xmm1, xmm6                         ;
addps xmm0, xmm1                         ; xmm0 = current position
mov eax,dest                             ;
movaps xmmword ptr [eax],xmm0            ; store current position
}
#else
const float state = movement->State();
const float complement = 1.0f - state;
for (int i = 0; i < 3; i = i + 1)
{
dest[i] = from[i]*state + to[i]*complement;
}
#endif
}
*/
void Triple::SquareDistance(Triple& a, Triple& b)
{
#ifdef SSSE3
	const void *dest = this->coord;
	__asm
	{
		mov eax,a
			movaps xmm0,xmmword ptr [eax]
		mov eax,b
			subps xmm0,xmmword ptr [eax]
		mulps xmm0, xmm0
			mov eax,dest
			movaps xmmword ptr [eax],xmm0
	}
#else
	for (int i = 0; i < 3; i += 1)
	{
		FP_DATATYPE d = a[i] - b[i];
		coord[i] = d*d;
	}
#endif
}

/*
void limit_triples_above(float *a, const float max)
{
#ifdef SSSE3
__asm
{
mov eax, a                     ;// load a
mov edx, eax                   ;// back up eax
movaps xmm0, [eax]             ;// 
add eax, 16                    ;// 
movaps xmm1, [eax]             ;// 
add eax, 16                    ;// 
movaps xmm2, [eax]             ;// 

movss xmm3, max                ;// load max
shufps xmm3, xmm3, 0x00        ;// 

minps xmm0, xmm3               ;// get minimum
minps xmm1, xmm3               ;// 
minps xmm2, xmm3               ;// 

movaps [edx], xmm0             ;// store a
add edx, 16                    ;// 
movaps [edx], xmm1             ;// 
add edx, 16                    ;// 
movaps [edx], xmm2             ;// 
}
#else
for (int i = 0; i < 12; i++)
{
if (a[i] > max)
{
a[i] = max;
}
}
#endif
}

void limit_triples_bellow_and_above(float *a, const float min, const float max)
{
#ifdef SSSE3
__asm
{
mov eax, a                     ;// load a
mov edx, eax                   ;// back up eax
movaps xmm0, [eax]             ;// 
add eax, 16                    ;// 
movaps xmm1, [eax]             ;// 
add eax, 16                    ;// 
movaps xmm2, [eax]             ;// 

movss xmm3, min                ;// load min
shufps xmm3, xmm3, 0x00        ;// 

maxps xmm0, xmm3               ;// get maximum
maxps xmm1, xmm3               ;// 
maxps xmm2, xmm3               ;// 

movss xmm3, max                ;// load max
shufps xmm3, xmm3, 0x00        ;// 

minps xmm0, xmm3               ;// get minimum
minps xmm1, xmm3               ;// 
minps xmm2, xmm3               ;// 

movaps [edx], xmm0             ;// store a
add edx, 16                    ;// 
movaps [edx], xmm1             ;// 
add edx, 16                    ;// 
movaps [edx], xmm2             ;// 
}
#else
for (int i = 0; i < 12; i++)
{
if (a[i] < min)
{
a[i] = min;
}
else
{
if (a[i] > max)
{
a[i] = max;
}
}
}
#endif
}
*/

void Triple::Normalize()
{
#ifdef SSSE3
	//<assembler using="eax, xmm0, xmm1"></assembler>
	__asm
	{
		mov eax, coord                 ;// start of square operation
		movups xmm0, [eax]             ;// unaligned pointer
		shufps xmm0, xmm0, 0x90        ;// 10010000 (shift all to the left, free position 0)
		mov eax, 0x00000000            ;// eax = 0.0f
		movss xmm0, eax                ;// position 0 = 0.0f

		movaps xmm1, xmm0              ;// save a copy

		mulps xmm0, xmm0               ;// square

		haddps xmm0, xmm0              ;// first horizontal sum (assuming position 0 is 0.0f)
		haddps xmm0, xmm0              ;// second horizontal sum

#ifndef SSE_APPROX_FP
		sqrtss xmm0, xmm0              ;// now we've got the length
		shufps xmm0, xmm0, 0x00        ;// spread length
		divps xmm1, xmm0               ;// compute normal coordinates
#else
		rsqrtss xmm0, xmm0             ;// now we've got the length reciprocal
		shufps xmm0, xmm0, 0x00        ;// spread length reciprocal
		mulps xmm1, xmm0               ;// compute normal coordinates
#endif
		mov eax, coord                 ;// begin store

		shufps xmm1, xmm1, 0xF9        ;// 11111001 (shift all to the right)
		movss [eax], xmm1              ;// 

		add eax, 4                     ;//
		shufps xmm1, xmm1, 0xF9        ;// 11111001 (shift all to the right)
		movss [eax], xmm1              ;//

		add eax, 4                     ;//
		shufps xmm1, xmm1, 0xF9        ;// 11111001 (shift all to the right)
		movss [eax], xmm1              ;//
	}
#else
	FP_DATATYPE length = sqrt(coord[X]*coord[X] + coord[Y]*coord[Y] + coord[Z]*coord[Z]);
	for (int i = 0; i < 3; i++)
	{
		coord[i] = coord[i] / length;
	}
#endif
}
